Efficient Trajectory Planning for Multiple Non-Holonomic Mobile Robots via Prioritized Trajectory Optimization

In this letter, we present a novel approach to efficiently generate collision-free optimal trajectories for multiple non-holonomic mobile robots in obstacle-rich environments. Our first employs graph-based multi-agent path planner find an initial discrete solution, and then refines solution into smooth using nonlinear optimization. We divide the robot team small groups propose prioritized trajectory optimization method improve scalability of algorithm. Infeasible sub-problems may arise some scenarios because decoupled framework. To handle problem, grouping priority assignment strategy is developed increase probability finding feasible trajectories. Compared coupled optimization, proposed reduces computation time considerably with impact on optimality plans. Simulations hardware experiments verified effectiveness superiority approach.